#ifndef __SV2_GIMBAL_TRACKING__
#define __SV2_GIMBAL_TRACKING__

#include "sms_core.h"
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <opencv2/opencv.hpp>

namespace sv2
{

  // 定义窗口名称
  static const std::string RGB_WINDOW = "Image window";

  // 鼠标点击事件的回调函数
  void onMouse(int event, int x, int y, int flags, void *);

  class GimbalTrackingCppNode : public sms::BaseNode
  {
  public:
    GimbalTrackingCppNode(
        std::string job_name,
        std::string param_file,
        std::string ip = "127.0.0.1",
        int port = 9094) : sms::BaseNode("GimbalTrackingCppNode", job_name, param_file, ip, port),
                           _det_vis_sub("/" + job_name + "/detection_vis", "sensor_msgs::CompressedImage", std::bind(&GimbalTrackingCppNode::full_vis_callback, this, std::placeholders::_1)),
                           _gimbal_state_sub("/" + job_name + "/gimbal/state", "spirecv_msgs::GimbalState", std::bind(&GimbalTrackingCppNode::gimbalstate_callback, this, std::placeholders::_1)),
                           _gimbal_ctl_pub("/" + job_name + "/gimbal/control", "spirecv_msgs::GimbalControl")
    {
      // 读取节点参数
      this->trackDef = this->get_param("trackDef", 50);
      this->tracked_id = this->get_param("tracked_id", 4);
      this->tracked_categories = this->get_param("tracked_categories", 0);
      // 定义一个窗口 才可以在上面操作
      cv::namedWindow(RGB_WINDOW);
      // 设置窗口操作回调函数，该函数实现吊舱控制
      cv::setMouseCallback(RGB_WINDOW, onMouse, 0);

      logger.info("trackDef: " + std::to_string(static_cast<double>(this->trackDef)));
      logger.info("tracked_id: " + std::to_string(static_cast<int>(this->tracked_id)));
      logger.info("tracked_categories: " + std::to_string(static_cast<int>(this->tracked_categories)));
    }
    ~GimbalTrackingCppNode()
    {
    }

    void run();

    void full_vis_callback(nlohmann::json msg)
    {
      // 放入到阻塞队列中
      {
        std::unique_lock<std::mutex> lock(this->_full_vis_queue_mtx);
        this->_full_vis_queue.push(msg);
      }
      // 通知主线程取数据
      this->_full_vis_cv.notify_one();
    }

    // gimbalctl参数话题回调函数
    void gimbalstate_callback(nlohmann::json msg)
    {
      this->gb_type = msg["gb_type"];
      this->move_mode = msg["move_mode"];
      this->is_zoom = msg["is_zoom"];
      this->rec_mode = msg["rec_mode"];

      this->angle_euler = msg["angle_euler"];
      this->angle_euler_rate = msg["angle_euler_rate"];
    }

    nlohmann::json  angle_euler;
    nlohmann::json  angle_euler_rate;
    int gb_type;
    int move_mode;
    int is_zoom;
    int rec_mode;

    double trackDef;
    int tracked_id;
    int tracked_categories;

  private:
    // 订阅话题
    sms::Subscriber _gimbal_state_sub;
    sms::Subscriber _det_vis_sub;
    // 发布话题
    sms::Publisher _gimbal_ctl_pub;

    std::mutex _full_vis_queue_mtx;
    std::queue<nlohmann::json> _full_vis_queue;
    std::condition_variable _full_vis_cv;
  };
}
#endif
